//
// Created by wubh2 on 2023/3/10.
//

#include "bsp_judge.h"
#include "string.h"
#include "math.h"
#include "protocol.h"
#include "driver_superc.h"
#include "bsp_uart.h"
#include "task_communicate.h"
#include "bsp_can.h"
#include "driver_chassis.h"
#include "task_lostcounter.h"
#include "driver_ui.h"


float b2f(uint8_t m0, uint8_t m1, uint8_t m2, uint8_t m3)
{
    // 求符号位
    float sig = 1.;

    if (m0 >= 128.)
        sig = -1.;
    //求阶码
    float jie = 0.;
    if (m0 >= 128.)
    {
        jie = m0 - 128.;
    }
    else
    {
        jie = m0;
    }
    jie = jie * 2.;
    if (m1 >= 128.)
        jie += 1.;
    jie -= 127.;
    //求尾码
    float tail = 0.;
    if (m1 >= 128.)
        m1 -= 128.;
    tail = m3 + (m2 + m1 * 256.) * 256.;
    tail = (tail) / 8388608;   //   8388608 = 2^23
    float f = sig * pow(2., jie) * (1 + tail);
    return f;
}




//----------------------------------------------协议处理-----------------------------------------------------------


/**
  * @brief  分数据，增加协议包改这里
  */

//裁判系统数据接收
/* data send (forward) */
/* data receive */
receive_judge_t judge_rece_mesg = {
        .game_robot_state_data.robot_id = BLUE_HERO_ID
};
ext_student_interactive_header_data_t student_interactive_header;
//-------------------------------车间通信接收代码-----------------------------------//
Sentry_communication_struct Sentry_comdata;
uint16_t len = 5;
uint8_t data_receive[5];


void robot_communicate_receive_data(uint16_t data_cmd_id, uint8_t* p_data, uint16_t len1)
{
    if (judge_rece_mesg.student_student_data.student_interactive_header_data.data_cmd_id == data_cmd_id)
    {
        for (int i = 0; i < len1; i++)
        {
            p_data[i] = judge_rece_mesg.student_student_data.robot_interactive_data.data[i];
        }
        Sentry_comdata.color = judge_rece_mesg.game_robot_state_data.robot_id > 100 ? 1 : 0;
        Sentry_comdata.sender = Sentry_comdata.color ? p_data[0] & 0x0f + 100 : p_data[0] & 0x0f;//反馈接收者编号
        Sentry_comdata.move_status = p_data[4];
        Sentry_comdata.lower_status = p_data[3];
        Sentry_comdata.uper_status = p_data[2];

        if (Sentry_comdata.sender == 7)//哨兵接收信号
        {
            switch (Sentry_comdata.move_status)//移动命令
            {
                case 0 ://自主控制

                    break;
                case 1: //停在右侧

                    break;
                case 2://停在中间

                    break;
                case 3 ://停在左侧
                    break;
                case 4 ://移动
                    break;
                case 5://逃脱

                    break;
                default :
                    break;
            }
            switch (Sentry_comdata.uper_status)
            {
                case 0:
                    break;
                case 1:
                    break;
            }

            Sentry_comdata.transmit_send_status = 5;
            Sentry_comdata.reciver = Sentry_comdata.color ? ((p_data[0] & 0xf0) >> 4) + 100 : (p_data[0] & 0xf0)
                    >> 4;//+100为了发送给蓝方
            Sentry_comdata.reactdata[0] = (Sentry_comdata.sender << 4) | Sentry_comdata.reciver;
            Sentry_comdata.reactdata[1] = 0;//反馈
            for (int i = 2; i < 5; i++)
                Sentry_comdata.reactdata[i] = p_data[i];

        }

    }
}

/**
  * @brief    get judgement system message
  */
extern enum REMOTESTATE RemoteControlMode;
extern u8 Do_heat_calibration;
u8 SupplyingFlag;
extern u8 DeathModeFlag;
extern float SmallGim_Heat_User;
extern float BigGimHeat_User;
int16_t SmalBulletSped = 0;
int16_t BigBulletSped = 0;
int16_t SmallBulletShotNum = 0;
int16_t BigBulletShotNum = 0;        //----------用来计算大子弹消耗数量
int16_t BigBulletShotNumLast = 0;
float BigBulletActualSpeed = 0;
u8 BigBulletIncreFlag = 0;
u8 BigBulletIncreFlag_2 = 0;
u8 NoBigBulletFlag = 0;
u8 AimingSequence[8] = { 0 };//瞄准顺序
extern float TargetBigBulletSpeed;//射击初速度上限
float savechassispower;
float Chassis_Power;
//-----------------------------------------给裁判系统打包发送的标准格式-----------------------------------------------------//
void judgeDataHandle(uint8_t* p_frame)
{
    frame_header_t* p_header = (frame_header_t*)p_frame;
    memcpy(p_header, p_frame, JUDGE_HEADER_LEN);

    uint16_t data_length = (p_frame[JUDGE_LEN_HIGH_INDEX] << 8) + p_frame[JUDGE_LEN_LOW_INDEX];
    uint16_t cmd_id = *(uint16_t*)(p_frame + JUDGE_HEADER_LEN);
    uint8_t* data_addr = p_frame + JUDGE_HEADER_AND_CMDID_LEN;

    switch (cmd_id)
    {
        case GAME_STATE_ID:
            memcpy(&judge_rece_mesg.game_state_data, data_addr, data_length);
            break;

        case GAME_RESULT_ID:
            memcpy(&judge_rece_mesg.game_result_data, data_addr, data_length);
            break;
        case GAME_SURVIVORS_ID://机器人血量数据
            memcpy(&judge_rece_mesg.ext_game_robot_HP_t, data_addr, data_length);
            break;
        case DART_STATUS_ID:
            memcpy(&judge_rece_mesg.dart_status_data, data_addr, data_length);
            break;
        case EVENT_DATA_ID:
            memcpy(&judge_rece_mesg.event_data, data_addr, data_length);
            break;
        case SUPPLY_ACTION_ID:
            memcpy(&judge_rece_mesg.supply_projectile_action_data, data_addr, data_length);
            if (judge_rece_mesg.supply_projectile_action_data.supply_robot_id ==
                judge_rece_mesg.game_robot_state_data.robot_id)
            {//如果是我在补弹
                SupplyingFlag = 1;
            }
            else
            {//如果不是我在补弹
                SupplyingFlag = 0;
            }
            break;
        case REFEREE_WARNING_ID://裁判系统警告ID
            memcpy(&judge_rece_mesg.referee_warning_data, data_addr, data_length);
            break;
        case DART_REMAINING_ID://飞镖发射口计时
            memcpy(&judge_rece_mesg.dart_remaining_data, data_addr, data_length);
            break;

        case GAME_ROBOT_STATE_ID://比赛机器人状态，10HZ
            savechassispower = judge_rece_mesg.power_heat_data.chassis_power;
            memcpy(&judge_rece_mesg.game_robot_state_data, data_addr, data_length);
            break;
        case POWER_HEAT_ID://实时功率热量数据，50HZ
            memcpy(&judge_rece_mesg.power_heat_data, data_addr, data_length);
            break;
        case ROBOT_POS_ID://机器人位置，10HZ
            memcpy(&judge_rece_mesg.game_robot_pos_data, data_addr, data_length);
            break;
        case BUFF_MUSK_ID://机器人增益，状态改变后发送
            memcpy(&judge_rece_mesg.buff_musk_data, data_addr, data_length);
            break;
        case AERIAL_ROBOT_ENERGY_ID:
            memcpy(&judge_rece_mesg.aerial_robot_energy_data, data_addr, data_length);
            break;
        case ROBOT_HURT_ID://伤害状态，伤害发生后发送
            memcpy(&judge_rece_mesg.robot_hurt_data, data_addr, data_length);
            break;
        case SHOOT_DATA_ID://实时射击信息，射击后发送
            memcpy(&judge_rece_mesg.shoot_data, data_addr, data_length);
            judge_rece_mesg.shoot_data.bullet_speed = b2f(data_addr[6], data_addr[5], data_addr[4], data_addr[3]);
            break;

        case STU_INTERACTIVE_ID://交互数据接收信息
            memcpy(&judge_rece_mesg.student_student_data, data_addr, data_length);
            robot_communicate_receive_data(0x02F0, data_receive, len);//接收
            break;
        case BULLET_REMAIN_ID://剩余子弹信息
            memcpy(&judge_rece_mesg.bullet_reamining_data, data_addr, data_length);
            break;
        case RFID_STATUS_ID://机器人RFID状态
            memcpy(&judge_rece_mesg.rfid_status_data, data_addr, data_length);
            break;
        case DART_CMD_ID://飞镖机器人客户端指令数据
            memcpy(&judge_rece_mesg.dart_cmd_data, data_addr, data_length);
            break;
        case CUSTOM_CONTROLLER_ID://自定义控制器交互数据接口
            memcpy(&judge_rece_mesg.custom_interactive_data, data_addr, data_length);
            break;
        case MINIMAP_DATA_ID://客户端小地图交互数据
            memcpy(&judge_rece_mesg.mini_map_data, data_addr, data_length);
            break;
        case KEYBORD_DATA_ID://键盘鼠标信息
            memcpy(&judge_rece_mesg.interactive_information_data, data_addr, data_length);
            break;

        default:
            break;
    }
    LostCounterFeed(JUDGEMENT_LOST_COUNT);
}


/**
  * @brief  打包
  */

uint8_t* protocol_packet_pack(uint16_t cmd_id, uint8_t* p_data, uint16_t len, uint8_t* tx_buf,
                              ext_student_interactive_header_data_t student_interactive_header)
{
    //memset(tx_buf, 0, 100);
    //static uint8_t seq;

    uint16_t frame_length = JUDGE_HEADER_AND_CMDID_LEN + len + JUDGE_CRC_LEN + JUDGE_INTERACTIVE_HEADER_LEN;
    frame_header_t* p_header = (frame_header_t*)tx_buf;

    p_header->sof = JUDGE_DN_REG_ID;
    p_header->data_length = len + 6;
    p_header->seq = 0;

    tx_buf[0] = JUDGE_DN_REG_ID;
    tx_buf[2] = (len + 6) >> 8;
    tx_buf[1] = (uint8_t)((len + 6) & 0xff);
    //p_header->seq          = seq++;
    memcpy(&tx_buf[JUDGE_HEADER_LEN], (uint8_t*)&cmd_id, JUDGE_CMDID_LEN);
    append_crc8_check_sum(tx_buf, JUDGE_HEADER_LEN);
    memcpy(&tx_buf[JUDGE_HEADER_AND_CMDID_LEN], (uint8_t*)&student_interactive_header, JUDGE_INTERACTIVE_HEADER_LEN);
    memcpy(&tx_buf[JUDGE_HEADER_AND_CMDID_LEN + JUDGE_INTERACTIVE_HEADER_LEN], p_data, len);
    append_crc16_check_sum(tx_buf, frame_length);

    return tx_buf;
}

uint8_t tx_buf[PROTOCAL_FRAME_MAX_SIZE];

//data_packet_pack(INFANTRY_SHOOT_MODE_ID, (uint8_t *)&gimbalmode,sizeof(gimbalmode), UP_REG_ID);

void data_packet_pack(uint16_t cmd_id, uint8_t* p_data, uint16_t len,
                      ext_student_interactive_header_data_t student_interactive_header)
{

    uint16_t frame_length = JUDGE_HEADER_AND_CMDID_LEN + len + JUDGE_CRC_LEN + JUDGE_INTERACTIVE_HEADER_LEN;   //5+2+4+2


    protocol_packet_pack(cmd_id, p_data, len, tx_buf, student_interactive_header);
    /* use mutex operation */
    //fifo_s_puts(&judge_txdata_fifo, tx_buf, frame_length);
    //for(uint16_t i = 0; i < frame_length; i++)
    //	xQueueSend(txQueue, &tx_buf[i], 0);
    usart_dma_send(&judgement_usart, tx_buf, frame_length);
  //  HAL_UART_Transmit(&huart6, tx_buf, frame_length,0xff);
}

//********************************车间通信发送代码**********************************//
void robot_communicate_send_data(uint16_t data_cmd_id, uint16_t send_ID, uint16_t receiver_ID, uint8_t* p_data,
                                 uint16_t len1)
{
    ext_student_interactive_header_data_t robot_interactive;
    robot_interactive.data_cmd_id = data_cmd_id;
    robot_interactive.send_ID = send_ID;
    robot_interactive.receiver_ID = receiver_ID;
    data_packet_pack(STU_INTERACTIVE_ID, p_data, len1, robot_interactive);
}

//---------------------------------------画图数据初始化-----------------------------------------//
ext_client_custom_graphic_delete_t Delete_Graph;
ext_client_custom_graphic_single_t Single_Graph;
ext_client_custom_graphic_double_t Double_Graph;
ext_client_custom_graphic_five_t Five_Graph;
ext_client_custom_graphic_seven_t Seven_Graph;
ext_client_custom_character_t Character_Graph;
u32 wiidth = 2;
u32 BodyStartX = 960;
u32 BodyStartY = 540;
u32 details_c = 0;
u32 BodyEndX = 0;
u32 BodyEndY = 0;
float float1 = 0;
//u32 Length   = 50;
//float Alpha  = 0;

uint8_t char_pitch_angle[10];
uint8_t char_bullet_speed[10];
uint8_t char_err_status[12];

//////////////车间通信部分
uint8_t calErrStatus(uint16_t err)
{
    for (uint8_t i = 0; i < NUMBERS_OF_COUNT; i++)
    {
        if ((err >> i) & 0x0001)
            return i;
    }

    return -1;
}

void Transform(float pitch_angle, uint8_t* char_pitch_angle)
{
    u8 i = 0;
    u8 j = 0;
    u8 k;
    uint8_t char_temper[10];
    int integer = (int)pitch_angle;
    int decimals = (int)((pitch_angle - integer) * 100);
    if (pitch_angle > 0)
    {
        integer = (int)pitch_angle;
        decimals = (int)((pitch_angle - integer) * 100);
        if (integer == 0)char_temper[i++] = '0';
        while (integer > 0)
        {
            char_temper[i++] = integer % 10 + '0';
            integer = integer / 10;
            j++;
        }
        for (k = 0; k < j; k++)
        {
            char_pitch_angle[k] = char_temper[j - k - 1];
        }
        char_pitch_angle[i++] = '.';
        char_temper[i++] = decimals % 10 + '0';
        decimals = decimals / 10;
        char_temper[i++] = decimals % 10 + '0';
        char_pitch_angle[i - 2] = char_temper[i - 1];
        char_pitch_angle[i - 1] = char_temper[i - 2];
        char_pitch_angle[i++] = 0;
    }
    else
    {
        i = 0;
        j = 0;
        pitch_angle = -1 * pitch_angle;
        integer = (int)pitch_angle;
        decimals = (int)((pitch_angle - integer) * 100);
        char_pitch_angle[i++] = '-';
        if (integer == 0)char_temper[i++] = '0';
        while (integer > 0)
        {
            char_temper[i++] = integer % 10 + '0';
            integer = integer / 10;
            j++;
        }
        for (k = 0; k < j; k++)
        {
            char_pitch_angle[k + 1] = char_temper[j - k];
        }
        char_pitch_angle[i++] = '.';
        char_temper[i++] = decimals % 10 + '0';
        decimals = decimals / 10;
        char_temper[i++] = decimals % 10 + '0';
        char_pitch_angle[i - 2] = char_temper[i - 1];
        char_pitch_angle[i - 1] = char_temper[i - 2];
        char_pitch_angle[i++] = 0;
    }
}

void Graph_test1(void)
{
    static u8 GraphEnemy_N = 0;
    static u8 Text_number = 0;//句子数量
    GraphEnemy_N++;
    //
    //GraphEnemy_N = 3;
    if (GraphEnemy_N < 6)
        Character_Graph.grapic_data_struct.operate_type = 1;
    else if (GraphEnemy_N < 11)
        Character_Graph.grapic_data_struct.operate_type = 2;
//		else if (GraphEnemy_N<3)//闪烁
//			Character_Graph.grapic_data_struct.operate_type  =3;
    else
        GraphEnemy_N = 0;
    Text_number++;
    Character_Graph.grapic_data_struct.graphic_type = 7;
    Character_Graph.grapic_data_struct.layer = 7;
    Character_Graph.grapic_data_struct.color = 3;
    Character_Graph.grapic_data_struct.details_a = 20;
    Character_Graph.grapic_data_struct.details_b = 9;
    Character_Graph.grapic_data_struct.width = 2;
    Character_Graph.grapic_data_struct.start_x = 20;
    Character_Graph.grapic_data_struct.details_c = 0;
    Character_Graph.grapic_data_struct.details_d = 0;
    Character_Graph.grapic_data_struct.details_e = 0;
    for (int i = 0; i < 30; i++)
        Character_Graph.data[i] = 0;
//		strcpy(Character_Graph.data,zero);
    switch (Text_number)
    {
        case 1:
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "1");
            Character_Graph.grapic_data_struct.start_y = 780;
            //第一句
            strcat(Character_Graph.data, "Hello infantry \n");
            break;
        case 2 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "2");
            Character_Graph.grapic_data_struct.start_y = 750;
            //第二句
            strcat(Character_Graph.data, " This is the console system\n");
            break;
        case 3 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "3");
            Character_Graph.grapic_data_struct.start_y = 720;
            //第三句
            strcat(Character_Graph.data, "  use keyboard to control me.");
            break;
        case 4 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "4");
            Character_Graph.grapic_data_struct.start_y = 690;
            //第三句
            strcat(Character_Graph.data, "   press ctrl+B continue.");
            break;
        case 5 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "5");
            Character_Graph.grapic_data_struct.start_y = 660;
            //第三句
            Character_Graph.grapic_data_struct.width = 5;
            strcat(Character_Graph.data, "  .........");
            break;
        default :
            Text_number = 0;
            break;
    }

    student_interactive_header.data_cmd_id = STU_CUSTOM_CHARACTER_ID;
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Character_Graph, sizeof(Character_Graph),
                     student_interactive_header);

}

void ClearAll(void)
{
    Delete_Graph.operate_type = 2;
    student_interactive_header.data_cmd_id = STU_CUSTOM_DELETE_PICTURE_ID;
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Delete_Graph, sizeof(Delete_Graph), student_interactive_header);
}

void AngleSet(double direction)
{
    static u8 GraphHurt_time = 0;
    double length = 100;//线长度
    GraphHurt_time++;

    if (GraphHurt_time < 2)
        for (int k = 0; k < 2; k++)
            Double_Graph.grapic_data_struct[k].operate_type = 1;
    else if (GraphHurt_time < 10)
        for (int k = 0; k < 2; k++)
            Double_Graph.grapic_data_struct[k].operate_type = 2;
    else
        GraphHurt_time = 0;

    strcpy(Double_Graph.grapic_data_struct[0].graphic_name, "I1");
    Double_Graph.grapic_data_struct[0].graphic_type = 2;
    Double_Graph.grapic_data_struct[0].layer = 1;
    Double_Graph.grapic_data_struct[0].color = 6;
    Double_Graph.grapic_data_struct[0].details_a = 0;
    Double_Graph.grapic_data_struct[0].details_b = 0;
    Double_Graph.grapic_data_struct[0].width = 7;
    Double_Graph.grapic_data_struct[0].start_x = 100;
    Double_Graph.grapic_data_struct[0].start_y = 300;
    Double_Graph.grapic_data_struct[0].details_c = 0;
    Double_Graph.grapic_data_struct[0].details_d = 100 + cos(direction) * 600;
    Double_Graph.grapic_data_struct[0].details_e = 300 + sin(direction + 0.03) * 600;


    strcpy(Double_Graph.grapic_data_struct[1].graphic_name, "I2");

    Double_Graph.grapic_data_struct[1].graphic_type = 0;
    Double_Graph.grapic_data_struct[1].layer = 1;
    Double_Graph.grapic_data_struct[1].color = 6;
    Double_Graph.grapic_data_struct[1].details_a = 0;
    Double_Graph.grapic_data_struct[1].details_b = 0;
    Double_Graph.grapic_data_struct[1].width = 20;
    Double_Graph.grapic_data_struct[1].start_x = 100;
    Double_Graph.grapic_data_struct[1].start_y = 300;
    Double_Graph.grapic_data_struct[1].details_c = 0;
    Double_Graph.grapic_data_struct[1].details_d = 100 + cos(direction) * length;
    Double_Graph.grapic_data_struct[1].details_e = 300 + sin(direction) * length;

    student_interactive_header.data_cmd_id = STU_CUSTOM_TWO_PICTURE_ID;
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Double_Graph, sizeof(Double_Graph), student_interactive_header);


}

void Graph_test2(void)//绘制准星及标尺
{
    for (int k = 0; k < 7; k++)
    {
        Seven_Graph.grapic_data_struct[k].operate_type = 1;
    }
    for (int k = 0; k < 5; k++)
    {
        Five_Graph.grapic_data_struct[k].operate_type = 1;
    }

//			}
    //第一个图形
    strcpy(Seven_Graph.grapic_data_struct[0].graphic_name, "C0");
    Seven_Graph.grapic_data_struct[0].graphic_type = 0;
    Seven_Graph.grapic_data_struct[0].layer = 8;
    Seven_Graph.grapic_data_struct[0].color = 3;
    Seven_Graph.grapic_data_struct[0].details_a = 0;
    Seven_Graph.grapic_data_struct[0].details_b = 0;
    Seven_Graph.grapic_data_struct[0].width = 2;
    Seven_Graph.grapic_data_struct[0].start_x = 960;
    Seven_Graph.grapic_data_struct[0].start_y = 190;
    Seven_Graph.grapic_data_struct[0].details_c = 0;
    Seven_Graph.grapic_data_struct[0].details_d = 960;
    Seven_Graph.grapic_data_struct[0].details_e = 540;


    //第二个图形
    strcpy(Seven_Graph.grapic_data_struct[1].graphic_name, "C1");
    Seven_Graph.grapic_data_struct[1].graphic_type = 0;
    Seven_Graph.grapic_data_struct[1].layer = 8;
    Seven_Graph.grapic_data_struct[1].color = 3;
    Seven_Graph.grapic_data_struct[1].details_a = 0;
    Seven_Graph.grapic_data_struct[1].details_b = 0;
    Seven_Graph.grapic_data_struct[1].width = 2;
    Seven_Graph.grapic_data_struct[1].start_x = 907;
    Seven_Graph.grapic_data_struct[1].start_y = 490;
    Seven_Graph.grapic_data_struct[1].details_c = 0;
    Seven_Graph.grapic_data_struct[1].details_d = 1013;
    Seven_Graph.grapic_data_struct[1].details_e = 490;
    //第三个图形
    strcpy(Seven_Graph.grapic_data_struct[2].graphic_name, "C2");

    Seven_Graph.grapic_data_struct[2].graphic_type = 0;
    Seven_Graph.grapic_data_struct[2].layer = 8;
    Seven_Graph.grapic_data_struct[2].color = 3;
    Seven_Graph.grapic_data_struct[2].details_a = 0;
    Seven_Graph.grapic_data_struct[2].details_b = 0;
    Seven_Graph.grapic_data_struct[2].width = 2;
    Seven_Graph.grapic_data_struct[2].start_x = 914;
    Seven_Graph.grapic_data_struct[2].start_y = 440;
    Seven_Graph.grapic_data_struct[2].details_c = 0;
    Seven_Graph.grapic_data_struct[2].details_d = 1006;
    Seven_Graph.grapic_data_struct[2].details_e = 440;
    //第四个图形
    strcpy(Seven_Graph.grapic_data_struct[3].graphic_name, "C3");
    Seven_Graph.grapic_data_struct[3].graphic_type = 0;
    Seven_Graph.grapic_data_struct[3].layer = 8;
    Seven_Graph.grapic_data_struct[3].color = 4;
    Seven_Graph.grapic_data_struct[3].details_a = 0;
    Seven_Graph.grapic_data_struct[3].details_b = 0;
    Seven_Graph.grapic_data_struct[3].width = 2;
    Seven_Graph.grapic_data_struct[3].start_x = 921;
    Seven_Graph.grapic_data_struct[3].start_y = 390;
    Seven_Graph.grapic_data_struct[3].details_c = 0;
    Seven_Graph.grapic_data_struct[3].details_d = 999;
    Seven_Graph.grapic_data_struct[3].details_e = 390;
    //第五个图形
    strcpy(Seven_Graph.grapic_data_struct[4].graphic_name, "C4");
    Seven_Graph.grapic_data_struct[4].graphic_type = 0;
    Seven_Graph.grapic_data_struct[4].layer = 8;
    Seven_Graph.grapic_data_struct[4].color = 3;
    Seven_Graph.grapic_data_struct[4].details_a = 0;
    Seven_Graph.grapic_data_struct[4].details_b = 0;
    Seven_Graph.grapic_data_struct[4].width = 2;
    Seven_Graph.grapic_data_struct[4].start_x = 928;
    Seven_Graph.grapic_data_struct[4].start_y = 340;
    Seven_Graph.grapic_data_struct[4].details_c = 0;
    Seven_Graph.grapic_data_struct[4].details_d = 992;
    Seven_Graph.grapic_data_struct[4].details_e = 340;
    //第六个图形  外框
    strcpy(Seven_Graph.grapic_data_struct[5].graphic_name, "C5");
    Seven_Graph.grapic_data_struct[5].graphic_type = 0;
    Seven_Graph.grapic_data_struct[5].layer = 8;
    Seven_Graph.grapic_data_struct[5].color = 3;
    Seven_Graph.grapic_data_struct[5].details_a = 0;
    Seven_Graph.grapic_data_struct[5].details_b = 0;
    Seven_Graph.grapic_data_struct[5].width = 2;
    Seven_Graph.grapic_data_struct[5].start_x = 617;
    Seven_Graph.grapic_data_struct[5].start_y = 0;
    Seven_Graph.grapic_data_struct[5].details_c = 0;
    Seven_Graph.grapic_data_struct[5].details_d = 694;
    Seven_Graph.grapic_data_struct[5].details_e = 111;
    //第七个图形  外框
    strcpy(Seven_Graph.grapic_data_struct[6].graphic_name, "C6");
    Seven_Graph.grapic_data_struct[6].graphic_type = 0;
    Seven_Graph.grapic_data_struct[6].layer = 8;
    Seven_Graph.grapic_data_struct[6].color = 3;
    Seven_Graph.grapic_data_struct[6].details_a = 0;
    Seven_Graph.grapic_data_struct[6].details_b = 0;
    Seven_Graph.grapic_data_struct[6].width = 2;
    Seven_Graph.grapic_data_struct[6].start_x = 1303;
    Seven_Graph.grapic_data_struct[6].start_y = 0;
    Seven_Graph.grapic_data_struct[6].details_c = 0;
    Seven_Graph.grapic_data_struct[6].details_d = 1226;
    Seven_Graph.grapic_data_struct[6].details_e = 111;


    student_interactive_header.data_cmd_id = STU_CUSTOM_SEVEN_PICTURE_ID;
    uint16_t len = sizeof(Seven_Graph);
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Seven_Graph, len, student_interactive_header);

}

extern Super_Cap_t superCap;
void Graph_test3(void)//电容条
{
//————————————————————————————————超级电容能能量条,两个图形——————————————————————————————//
    static u8 GraphFlag3_time = 0;
    GraphFlag3_time++;
    if (GraphFlag3_time < 2)
        for (int k = 0; k < 2; k++)
            Double_Graph.grapic_data_struct[k].operate_type = 1;
    else if (GraphFlag3_time < 10)
        for (int k = 0; k < 2; k++)
            Double_Graph.grapic_data_struct[k].operate_type = 2;
    else
        GraphFlag3_time = 0;

    strcpy(Double_Graph.grapic_data_struct[0].graphic_name, "s1");
    Double_Graph.grapic_data_struct[0].graphic_type = 1;
    Double_Graph.grapic_data_struct[0].layer = 6;
    if (chassisControlData.superFlag)
        Double_Graph.grapic_data_struct[0].color = 0;
    else
        Double_Graph.grapic_data_struct[0].color = 7;
    Double_Graph.grapic_data_struct[0].details_a = 0;
    Double_Graph.grapic_data_struct[0].details_b = 0;
    Double_Graph.grapic_data_struct[0].width = 10;
    Double_Graph.grapic_data_struct[0].start_x = 800;
    Double_Graph.grapic_data_struct[0].start_y = 35;
    Double_Graph.grapic_data_struct[0].details_c = 0;
//		if(SuperCVoltage>18)
//		  Double_Graph.grapic_data_struct[0].details_d			   = ((SuperCVoltage-18)/24)*345;
//		else
    Double_Graph.grapic_data_struct[0].details_d = 800 + ( superCap.scFeedback.V_SC - 11.5) * 345 / 12; //11.5~23.5
    if (Double_Graph.grapic_data_struct[0].details_d < 800) Double_Graph.grapic_data_struct[0].details_d = 800;
    Double_Graph.grapic_data_struct[0].details_e = 25;


    strcpy(Double_Graph.grapic_data_struct[1].graphic_name, "s2");

    Double_Graph.grapic_data_struct[1].graphic_type = 1;
    Double_Graph.grapic_data_struct[1].layer = 6;
    if (chassisControlData.superFlag)
        Double_Graph.grapic_data_struct[1].color = 0;
    else
        Double_Graph.grapic_data_struct[1].color = 7;
    Double_Graph.grapic_data_struct[1].details_a = 0;
    Double_Graph.grapic_data_struct[1].details_b = 0;
    Double_Graph.grapic_data_struct[1].width = 2;
    Double_Graph.grapic_data_struct[1].start_x = 800;
    Double_Graph.grapic_data_struct[1].start_y = 40;
    Double_Graph.grapic_data_struct[1].details_c = 0;
    Double_Graph.grapic_data_struct[1].details_d = 1150;
    Double_Graph.grapic_data_struct[1].details_e = 20;

    student_interactive_header.data_cmd_id = STU_CUSTOM_TWO_PICTURE_ID;
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Double_Graph, sizeof(Double_Graph), student_interactive_header);
}

//void err_str_cat(uint8_t err_code)
//{

//    Character_Graph.grapic_data_struct.color = 2;
//    switch (err_code) {
//        case JUDGEMENT_LOST_COUNT:
//            strcat(Character_Graph.data, "JUDGEMENT"); break;
//        case FEEDMOTOR_LOST_COUNT:
//            strcat(Character_Graph.data, "FEEDMOTOR"); break;
//        case CHASSIS_MOTOR_0:
//            strcat(Character_Graph.data, "CHASSIS_0"); break;
//        case CHASSIS_MOTOR_1:
//            strcat(Character_Graph.data, "CHASSIS_1"); break;
//        case CHASSIS_MOTOR_2:
//            strcat(Character_Graph.data, "CHASSIS_2"); break;
//        case CHASSIS_MOTOR_3:
//            strcat(Character_Graph.data, "CHASSIS_3"); break;
//        case L_FRICTION_LOST_COUNT:
//            strcat(Character_Graph.data, "L_FRICTION"); break;
//        case R_FRICTION_LOST_COUNT:
//            strcat(Character_Graph.data, "R_FRICTION"); break;
//        case H_FRICTION_LOST_COUNT:
//            strcat(Character_Graph.data, "H_FRICTION"); break;
//        case GIMBAL_MOTOR_PITCH:
//            strcat(Character_Graph.data, "PITCH"); break;
//        case GIMBAL_MOTOR_YAW:
//            strcat(Character_Graph.data, "YAW"); break;
//        case SUPERC_LOST_COUNT:
//            strcat(Character_Graph.data, "SUPERC"); break;
//        default:
//            Character_Graph.grapic_data_struct.color = 3;
//            strcat(Character_Graph.data, "OK");
//            break;
//    }
//}
char err_state1[12][12]={"JUDGEMENT","FEEDMOTOR","CHASSIS_0","CHASSIS_1","CHASSIS_2","CHASSIS_3","L_FRICTION","R_FRICTION","H_FRICTION","PITCH","YAW","SUPERC"};
void err_str_cat(uint8_t err_code){
    Character_Graph.grapic_data_struct.color = 2;
    int flag = FALSE;
    for(int i = 0;i < 12;i++){
        if(err_code == i + 1) {
            strcat(Character_Graph.data, err_state1[i]);
            flag = TRUE;
        }
    }
    if(flag == FALSE){
        Character_Graph.grapic_data_struct.color = 3;
        strcat(Character_Graph.data,"OK");
    }
}

void GraphTest4(int8_t err_code, u8 chassis)//,u8 speed,u8 power)射速和功率后续添加
{
    static u8 GraphFlag2_Time = 0;
    static u8 Text_number = 0;//句子数量

    GraphFlag2_Time++;
    if (GraphFlag2_Time < 2)
        Character_Graph.grapic_data_struct.operate_type = 1;
    else if (GraphFlag2_Time < 10)
        Character_Graph.grapic_data_struct.operate_type = 2;
//		else if (GraphFlag8_Time<3)//闪烁
//			Character_Graph.grapic_data_struct.operate_type  =3;
    else
        GraphFlag2_Time = 0;
    Text_number = (Text_number + 1) % 7;
    Character_Graph.grapic_data_struct.graphic_type = 7;
    Character_Graph.grapic_data_struct.layer = 5;
    Character_Graph.grapic_data_struct.color = 3;
    Character_Graph.grapic_data_struct.details_a = 20;
    Character_Graph.grapic_data_struct.details_b = 9;
    Character_Graph.grapic_data_struct.width = 2;
    Character_Graph.grapic_data_struct.start_x = 20;
    Character_Graph.grapic_data_struct.details_c = 0;
    Character_Graph.grapic_data_struct.details_d = 0;
    Character_Graph.grapic_data_struct.details_e = 0;
    for (int i = 0; i < 30; i++)
        Character_Graph.data[i] = 0;
//		strcpy(Character_Graph.data,zero);

    switch (Text_number)
    {
        case 1 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "A2");
            Character_Graph.grapic_data_struct.start_y = 750;
            //第二句
            strcat(Character_Graph.data, "Err\t");
            err_str_cat(err_code);

            break;
        case 2 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "A3");
            Character_Graph.grapic_data_struct.start_y = 710;
            strcat(Character_Graph.data, "Chassis:\t");//还要添加小陀螺和flag45度
            if (chassis == 0)
                strcat(Character_Graph.data, "F");//跟随
            else if (chassis == 1)
            {
                Character_Graph.grapic_data_struct.color = 6;            //青色
                strcat(Character_Graph.data, "S");//小陀螺
            }
            else if (chassis == 2)
            {
                Character_Graph.grapic_data_struct.color = 4;            //紫红色
                strcat(Character_Graph.data, "L");//自锁
            }
            else if (chassis == 3)
            {
                Character_Graph.grapic_data_struct.color = 8;            //白色
                strcat(Character_Graph.data, "L");//打符
            }
            //第三句
            break;
        case 3 :
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "a5");
            Character_Graph.grapic_data_struct.start_y = 630;
            strcat(Character_Graph.data, "Fric-Auto:\t");

            if ((gimbalStatus.chassisData.heroStatus % 4) == 0)
                strcat(Character_Graph.data, "Off\t");
            else if ((gimbalStatus.chassisData.heroStatus % 4) == 1)
                strcat(Character_Graph.data, "R1\t");
            else if ((gimbalStatus.chassisData.heroStatus % 4) == 2)
                strcat(Character_Graph.data, "R2\t");
            else
                strcat(Character_Graph.data, "???\t");

            if ((gimbalStatus.chassisData.heroStatus >> 2) == 0)
                strcat(Character_Graph.data, "Off");
            else if ((gimbalStatus.chassisData.heroStatus >> 2) == 1)
                strcat(Character_Graph.data, "On");
            else if ((gimbalStatus.chassisData.heroStatus >> 2) == 2)
                strcat(Character_Graph.data, "AutoShoot");
            else
                strcat(Character_Graph.data, "???");

            break;
        default:
            strcpy(Character_Graph.grapic_data_struct.graphic_name, "a4");
            Character_Graph.grapic_data_struct.start_y = 670;
            strcat(Character_Graph.data, "Pitch Angle:\t");
            strcat(Character_Graph.data, char_pitch_angle);
            strcat(Character_Graph.data, " \n");
            break;
    }

    student_interactive_header.data_cmd_id = STU_CUSTOM_CHARACTER_ID;
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Character_Graph, sizeof(Character_Graph),
                     student_interactive_header);
}

void Graph_test5(void)//绘制准星及标尺
{
    static u8 Text_number = 0;//句子数量
    Text_number++;

    for (int k = 0; k < 5; k++)
    {
        Single_Graph.grapic_data_struct.operate_type = 1;
    }


    switch (Text_number)
    {
        case 1:
            //  	//第八个图形
            strcpy(Single_Graph.grapic_data_struct.graphic_name, "C7");
            Single_Graph.grapic_data_struct.graphic_type = 0;
            Single_Graph.grapic_data_struct.layer = 9;
            Single_Graph.grapic_data_struct.color = 3;
            Single_Graph.grapic_data_struct.details_a = 0;
            Single_Graph.grapic_data_struct.details_b = 0;
            Single_Graph.grapic_data_struct.width = 2;
            Single_Graph.grapic_data_struct.start_x = 935;
            Single_Graph.grapic_data_struct.start_y = 290;
            Single_Graph.grapic_data_struct.details_c = 0;
            Single_Graph.grapic_data_struct.details_d = 985;
            Single_Graph.grapic_data_struct.details_e = 290;

            break;
        case 2 :
            //第九个图形
            strcpy(Single_Graph.grapic_data_struct.graphic_name, "C8");
            Single_Graph.grapic_data_struct.graphic_type = 0;
            Single_Graph.grapic_data_struct.layer = 9;
            Single_Graph.grapic_data_struct.color = 3;
            Single_Graph.grapic_data_struct.details_a = 0;
            Single_Graph.grapic_data_struct.details_b = 0;
            Single_Graph.grapic_data_struct.width = 2;
            Single_Graph.grapic_data_struct.start_x = 942;
            Single_Graph.grapic_data_struct.start_y = 240;
            Single_Graph.grapic_data_struct.details_c = 0;
            Single_Graph.grapic_data_struct.details_d = 978;
            Single_Graph.grapic_data_struct.details_e = 240;
            break;
        case 3 :
            //第十个图形
            strcpy(Single_Graph.grapic_data_struct.graphic_name, "C9");
            Single_Graph.grapic_data_struct.graphic_type = 0;
            Single_Graph.grapic_data_struct.layer = 9;
            Single_Graph.grapic_data_struct.color = 3;
            Single_Graph.grapic_data_struct.details_a = 0;
            Single_Graph.grapic_data_struct.details_b = 0;
            Single_Graph.grapic_data_struct.width = 2;
            Single_Graph.grapic_data_struct.start_x = 948;
            Single_Graph.grapic_data_struct.start_y = 190;
            Single_Graph.grapic_data_struct.details_c = 0;
            Single_Graph.grapic_data_struct.details_d = 971;
            Single_Graph.grapic_data_struct.details_e = 190;
            break;
        default :
            Text_number = 0;
            break;
    }
    student_interactive_header.data_cmd_id = STU_CUSTOM_ONE_PICTURE_ID;
    data_packet_pack(STU_INTERACTIVE_ID, (uint8_t*)&Single_Graph, sizeof(Single_Graph), student_interactive_header);

}


u8 DrawFlag = 1;
uint8_t clearFlag = 1;
u8 HurtFlag = 0;
double Direc = 0;
uint8_t data_send[5] = { 0x37, 5 };

void Graph_send(void)
{
    //Graph_Init();
    u8 gimbal4;
    u8 chassis4;

    if (clearFlag)
    {
        ClearAll();
        clearFlag = 0;
        return;
    }

    DrawFlag = (DrawFlag + 1) % 20;
    if (DrawFlag == 0)
    {
        Graph_test2();//绘制准星    7个图
    }
    else if (DrawFlag == 1)
    {
        Graph_test5();//绘制准星
    }
    else if (DrawFlag < 5)
    {
        Graph_test3();//画电容条    2个图
    }
    else
    {
        // 左侧字符
        Transform((float)(gimbalStatus.chassisData.angle) / 100.0f, char_pitch_angle);

        int8_t err_code = calErrStatus(GetErrorState());

        if (chassisControlData.chassisMode == CHASSIS_MODE_GYRO) chassis4 = 1;
        else if (chassisControlData.chassisMode == CHASSIS_MODE_LOCK) chassis4 = 3;//作为是否开启狙击模式的标志
        else if (chassisControlData.chassisMode == CHASSIS_MODE_FOLLOW)chassis4 = 0;//作为是否跟随的标志);
        else chassis4 = 2;
        GraphTest4(err_code, chassis4);     //左侧模式加入云台0关闭1a摩擦轮2弹仓盖和底盘0跟随1陀螺2十五度3打符
    }
}

uint8_t uart5TxBuf[1024];

void StuInteractiveData(void)
{
    if (judge_rece_mesg.game_robot_state_data.robot_id == BLUE_HERO_ID)//蓝1
    {
        student_interactive_header.send_ID = BLUE_HERO_ID;
        student_interactive_header.receiver_ID = BLUE_HERO_CUSTOM_ID;
//		Graph_send();
        Draw_UI(!clearFlag);
    }
    else if (judge_rece_mesg.game_robot_state_data.robot_id == RED_HERO_ID)//红1
    {
        student_interactive_header.send_ID = RED_HERO_ID;
        student_interactive_header.receiver_ID = RED_HERO_CUSTOM_ID;
//		Graph_send();
        Draw_UI(!clearFlag);
    }
    else if (judge_rece_mesg.game_robot_state_data.robot_id == BLUE_INFANTRY5_ID)//蓝五
    {
        student_interactive_header.send_ID = BLUE_INFANTRY5_ID;
        student_interactive_header.receiver_ID = BLUE_INFANTRY5_CUSTOM_ID;
        Graph_send();//
    }
    else if (judge_rece_mesg.game_robot_state_data.robot_id == RED_INFANTRY3_ID)//红3
    {
        student_interactive_header.send_ID = RED_INFANTRY3_ID;
        student_interactive_header.receiver_ID = RED_INFANTRY3_CUSTOM_ID;
        Graph_send();
//			SentryControl(3);
        robot_communicate_send_data(0x02F0, RED_INFANTRY3_ID, RED_SENTRY_ID, data_send, 5);//发送（发送给哨兵）

    }
    else if (judge_rece_mesg.game_robot_state_data.robot_id == RED_INFANTRY4_ID)//红四
    {
        student_interactive_header.send_ID = RED_INFANTRY4_ID;
        student_interactive_header.receiver_ID = RED_INFANTRY4_CUSTOM_ID;
        Graph_send();//
    }
    else if (judge_rece_mesg.game_robot_state_data.robot_id == RED_INFANTRY5_ID)//红五
    {
        student_interactive_header.send_ID = RED_INFANTRY5_ID;
        student_interactive_header.receiver_ID = RED_INFANTRY5_CUSTOM_ID;
        Graph_send();//
    }

    /*uint16_t p = 0, txLen = uxQueueMessagesWaiting(txQueue);
    while(uxQueueMessagesWaiting(txQueue) > 0)
    {
        xQueueReceive(txQueue, &uart5TxBuf[p++], 0);
    }
    if(txLen > 0)
    usart_dma_send(&judgement_usart, uart5TxBuf, txLen);
    */
    //osSignalSet(unpackTaskHandle, UART_TX_SIGNAL);
}